
#include <string>
#include <sstream>
#include <pcl/io/pcd_io.h>
#include "keypoints.h"
#include "functions.h"
#include "load.h"
#include <time.h>
#include <stdlib.h>
#include "narf.h"
#include "registration.h"



using namespace std;
using namespace pcl;





int main(int argc, char *argv[])
{
    cout<<"#############################"<<endl;
    cout<<"######## Starting... ########"<<endl;
    cout<<"#############################"<<endl;
    //CANCELLA WARNING PCL
    console::setVerbosityLevel(console::L_ALWAYS);

    PointCloud<PointXYZ>::Ptr object(new PointCloud<PointXYZ>);
    PointCloud<PointXYZ>::Ptr world(new PointCloud<PointXYZ>);

    stringstream ssObject;
    stringstream ssWorld;


    string path="../dataset-arm/scans/";
    char objectType = argv[1][0];
    string folder = argv[2];
    ssWorld <<path<<folder<<"/points.pcd";
    switch (objectType)
    {
    case 'A':
        ssObject <<path<<"modelA.pcd";
        break;
    case 'B':
        ssObject <<path<<"modelB.pcd";
        break;
    case 'C':
        ssObject <<path<<"modelC_cut.pcd";//"modelC.pcd";"modelC2.pcd";"modelC_cut.pcd";
        break;
    case 'D':
        ssObject <<path<<"modelD.pcd";
        break;


    }
    /*OLD DATASET
    string objectType = argv[1];// 2d o 3d
    string folder = argv[2];//05-12 per 3d 15-22 per 2d
    if (objectType.compare("3d")==0)
    {
        ssWorld <<"../edmond/scans/lamiera3d/"<<folder<<"/points.pcd";
        ssObject <<"../edmond/scans/lamiera3d/model_cut.pcd";//model(_really)_cut.pcd
    }
    else if (objectType.compare("2d")==0)
    {
        ssWorld <<"../edmond/scans/lamiera2d/"<<folder<<"/points.pcd";
        ssObject <<"../edmond/scans/lamiera2d/model.pcd";
    }
    else
    {
        ssWorld <<"../edmond/scans/spinotti/"<<folder<<"/points.pcd";
        ssObject <<"../edmond/scans/spinotti/model_cut.pcd";
    }*/

    cerr<<"Processing file ... "<<ssWorld.str()<<endl;
    regConfig config;

    fstream file("config", ios::in);
    vector<string> lines;
    int j=0;
    while ( file.good() )
    {
        string x;
        lines.push_back(x);
        getline (file,lines.at(j++));
    }

    vector<string> values;
    for (j=0;j<lines.size();j++)
    {

        stringstream currentLine;
        if(lines.at(j).size()==0) break;
        currentLine << lines.at(j);
        string s;
        vector< string> tokenizedLine;
        while (getline(currentLine, s, ' '))
        {
            tokenizedLine.push_back(s);
        }
        values.push_back(tokenizedLine.at(1));
    }

    config.inputVoxelResolution=atof(values.at(0).c_str());//computeLeafSize(target,1000)*0.5f;//<-------- RESOLUTION MUST BE > THAN PCD RESOLUTION OR 0
    config.clusterThreshold=config.inputVoxelResolution*atof(values.at(1).c_str());//soglia clustering 10% piu' di risoluzione di input
    config.minClusterSize = atof(values.at(2).c_str());
    config.maxClusterSize = atof(values.at(3).c_str());
    config.initialAlgorithm=atoi(values.at(4).c_str());//Algorithm 1 or 2
    config.initialScoreThreshold=atof(values.at(5).c_str()); //soglia limite di euclidianScore per considerare buona una registrazione iniziale
    config.finalScoreThreshold=atof(values.at(6).c_str()); //soglia limite di percentScore per considerare buona una registrazione finale
    config.initialResolution=atof(values.at(7).c_str());//risoluzione per trasf iniziale
    config.finalResolution=atof(values.at(8).c_str());//risoluzione per trasf finale
    config.minRadiusFPFH=atof(values.at(9).c_str());//minimo raggio FPFH provato
    config.maxRadiusFPFH=atof(values.at(10).c_str());//massimo raggio FPFH provato
    config.stepRadiusFPFH=atof(values.at(11).c_str());//intervallo di prova per i raggi FPFH
    config.minSACThresh=atof(values.at(12).c_str());//minima soglia SAC provata
    config.maxSACThresh=atof(values.at(13).c_str());//massima soglia SAC provata
    config.stepSACThresh=atof(values.at(14).c_str());//intervallo di prova per i le SAC thresholds
    config.showViewer=atoi(values.at(15).c_str());
    config.parallel=atoi(values.at(16).c_str());
    config.segmentPlane=atoi(values.at(17).c_str());

    time_t start,end; time (&start);
    loadCloud(ssWorld.str(),world,config.inputVoxelResolution,config.segmentPlane,1);
    loadCloud(ssObject.str(),object,config.inputVoxelResolution,0,0);
    time (&end); double dif = difftime (end,start);
    cout << "------------------------------------->  Clouds loaded in " << dif << " seconds"<< endl;


    //MULTIPLE REGISTRATION WITH ALIGNMENT CLASS
    vector<Alignment*> alignmentAttempts;
    PointCloud<PointXYZI>::Ptr object_keypoints (new PointCloud<PointXYZI> );

       registerAllWithAlignment(object,world,alignmentAttempts,object_keypoints,config);

    if (config.showViewer)
    {
        //VIEWER
        visualization::PCLVisualizer viewer("PCL Viewer");
        int v1(1);
        int v2(2);
        int v3(3);
        viewer.createViewPort(0,0.5,0.5,1,v1);
        viewer.createViewPort(0.5,0.5,1,1,v3);
        viewer.createViewPort(0,0,1,0.5,v2);
        viewer.setBackgroundColor (1, 1, 1);
        viewer.initCameraParameters ();
        viewer.addText("InitialAlignment",0,0,15,255,255,255,"Initial",v1);
        viewer.addText("Clusters",0,0,15,255,255,255,"Clusters",v3);
        viewer.addText("FinalAlignment",0,0,15,255,255,255,"Final",v2);
        //-------------------------------CLUSTERS
        vector<PointIndices> clusters = findClusters(*world,config.clusterThreshold,object->size()*config.minClusterSize,object->size()*config.maxClusterSize);
        for(int i=0;i<clusters.size();i++)
        {
            stringstream ssCluster;
            ssCluster<<"Cluster"<<i;
            PointCloud<PointXYZ>::Ptr cluster(new PointCloud<PointXYZ>);
            copyPointCloud(*world,clusters.at(i).indices,*cluster);
            visualization::PointCloudColorHandlerCustom<PointXYZ> rgbCluster(cluster,rand()%255,rand()%255,rand()%255);
            viewer.addPointCloud<PointXYZ> (cluster, rgbCluster, ssCluster.str(),v3);
        }
        //-------------------------------INITIAL
        //object
        visualization::PointCloudColorHandlerCustom<PointXYZ> rgbObject(object,0,0,255);
        viewer.addPointCloud<PointXYZ> (object, rgbObject, "Object",v1);
        visualization::PointCloudColorHandlerCustom<PointXYZI> rgbObject_keypoints(object_keypoints,0,0,255);
        viewer.addPointCloud<PointXYZI> (object_keypoints, rgbObject_keypoints, "Object_keypoints",v1);
        viewer.setPointCloudRenderingProperties(visualization::PCL_VISUALIZER_POINT_SIZE, 2, "Object_keypoints");
        //world
        visualization::PointCloudColorHandlerCustom<PointXYZ> rgbWorld(world,0,0,0);
        viewer.addPointCloud<PointXYZ> (world, rgbWorld, "World1",v1);
        visualization::PointCloudColorHandlerCustom<PointXYZ> rgbWorld2(world,0,0,0);
        viewer.addPointCloud<PointXYZ> (world, rgbWorld2, "World2",v2);
        for(int i=0;i<alignmentAttempts.size();i++)
        {
            ssObject.str(""); ssWorld.str(""); stringstream ssCorrespond; stringstream ssObjectRegistered;
            ssObject<<"Object"<<i; ssWorld<<"World_keypoints"<<i; ssCorrespond <<"Correspond"<<i; ssObjectRegistered <<"ObjectRegistered"<<i;
            //object
            int red=round(alignmentAttempts.at(i)->getInitialScore()/config.initialScoreThreshold*255.0f);
            int green=255-red;
            visualization::PointCloudColorHandlerCustom<PointXYZ> rgbInitialObjectRegistered(alignmentAttempts.at(i)->getInitialObjectRegistered(),red,green,0);//0,200,255);
            viewer.addPointCloud<PointXYZ> (alignmentAttempts.at(i)->getInitialObjectRegistered(), rgbInitialObjectRegistered, ssObject.str(),v1);
            //world
            visualization::PointCloudColorHandlerCustom<PointXYZI> rgbWorld_keypoints(alignmentAttempts.at(i)->getWorld_keypoints(),rand()%3*50,rand()%3*50,rand()%3*50);
            viewer.addPointCloud<PointXYZI> (alignmentAttempts.at(i)->getWorld_keypoints(), rgbWorld_keypoints, ssWorld.str(),v1);
            viewer.setPointCloudRenderingProperties(visualization::PCL_VISUALIZER_POINT_SIZE, 2, ssWorld.str());
            viewer.addCorrespondences<PointXYZI>(object_keypoints,alignmentAttempts.at(i)->getWorld_keypoints(),*( alignmentAttempts.at(i)->getCorrespond() ), ssCorrespond.str(),v1);
            //-------------------------------FINAL
            red=round(alignmentAttempts.at(i)->getFinalScore()/config.finalScoreThreshold*255.0f);
            green=255-red;
            visualization::PointCloudColorHandlerCustom<PointXYZ> rgbObjectRegistered(alignmentAttempts.at(i)->getObjectRegistered(),red,green,0);
            viewer.addPointCloud<PointXYZ> (alignmentAttempts.at(i)->getObjectRegistered(), rgbObjectRegistered, ssObjectRegistered.str(),v2);
        }
        viewer.resetCamera();
        while (!viewer.wasStopped ())	{ viewer.spin ();}
        //END VIEWER
    }
    for(int i=0;i<alignmentAttempts.size();i++)
        cerr <<"Object "<< i << " found: Initial Score: "<< alignmentAttempts.at(i)->getInitialScore()<< " / Final Score: "<< alignmentAttempts.at(i)->getFinalScore()<<endl;

    cout<<"OK!!"<<endl;
    return(0);


}//END MAIN










